/*
 * CoreC.c
 *
 * Created: 25/03/2014 15:27:28
 *  Author: Jason
 */ 

#include <util/delay.h>
#include <avr/io.h>
#include <avr/wdt.h>
#include <avr/interrupt.h>
#include "twistuff.c"

#include "CoreC.h"

#define lineMidPoint 50

volatile lineSensorData_type lineSensorData;

volatile wallSensorData_type wallSensorData;

volatile buttonData_type buttonData;
volatile LEDData_type LEDData;
volatile config_type config;
volatile mode_type mode;
volatile stringData_type OLEDString;
volatile stringData_type RFOutString;
volatile stringData_type RFInString;
volatile RobotSpeed_type robotSpeed;
volatile URobotDistance_type URobotDistance;

uint8_t GO = 0;
//uint8_t wallCalMin = 0;
//uint8_t wallcalMax = 255;
//uint8_t lineCalMin = 0;
//uint8_t lineCalMax = 255;


//uint8_t  simdata = 0;
int main(void)
{
  WDT_off(); //disable watchdog NOW so we don't get reset
  sei();   // Enable Global Interrupts
  _delay_ms(5000); //wait for Arduino to boot 
  mode.robotFunction = ROBOTFUNCTION_WAITFORUSER;

  setupMillis();
  
  HCISetup();
  
  i2cSetup();
  
  //OLEDSetup();

  //MouseModeSetup();
  //SPISlaveSetup();
  //RFSetup();
  WallSensorsSetup();
  LineSensorsSetup();

  //
  //
  //robotSpeed.LeftSpeed = simdata;
  //robotSpeed.LeftSpeed = URobotDistance.robotDistance.LeftDistance & 0xff;
  
  while(1)
  {
    HCILoop();

  //_delay_ms(20); 
    //readAllI2CData();
      _delay_ms(20); 
    writeAllI2CData();
    //blink(1<<LED1,1,blinkSpeed_125,blinkSpeed_125);
    
    if (buttonData.dataFresh)
    {

      switch (buttonData.btnPressedEvent)
      {
        case HCIDATA_BUTTONPRESSEDEVENT_BUTTON1_SHORTPRESS:
        //cycle through functions

          mode.robotFunction++;
          if (mode.robotFunction > NO_OF_ROBOTFUNCTIONS) {mode.robotFunction = 1;}
            _delay_ms(1000);
          blink((1<<LED1|1<<LED2),mode.robotFunction,blinkSpeed_250,blinkSpeed_125);
          _mode_init();
        break;
        case HCIDATA_BUTTONPRESSEDEVENT_BUTTON1_LONGPRESS:
        break;
        case HCIDATA_BUTTONPRESSEDEVENT_BUTTON2_SHORTPRESS:
          //if (mode.robotFunction == ROBOTFUNCTION_DRAGRACE || mode.robotFunction == ROBOTFUNCTION_LINEFOLLOWER)
          //{
            //lineMidPoint = lineCalMin + ((lineCalMax-lineCalMin)/2);
          //}
          GO = 1;
        break;
        case HCIDATA_BUTTONPRESSEDEVENT_BUTTON2_LONGPRESS:
        break;
      }
      buttonData.dataFresh = 0;
    }

    switch(mode.robotFunction)
    {
      case ROBOTFUNCTION_WAITFORUSER:
      break;

      case ROBOTFUNCTION_DRAGRACE:
        readLineSensors();

      if (GO)
      {
        DragGo();
      }
      else
      {
        //calibrate min and max
        //cli();
        //if (lineSensorData.sensor1 > lineCalMax){lineCalMax=lineSensorData.sensor1;}
        //if (lineSensorData.sensor2 > lineCalMax){lineCalMax=lineSensorData.sensor2;}
        //if (lineSensorData.sensor3 > lineCalMax){lineCalMax=lineSensorData.sensor3;}
        //if (lineSensorData.sensor4 > lineCalMax){lineCalMax=lineSensorData.sensor4;}
        //if (lineSensorData.sensor5 > lineCalMax){lineCalMax=lineSensorData.sensor5;}
        //if (lineSensorData.sensor1 < lineCalMin){lineCalMin=lineSensorData.sensor1;}
        //if (lineSensorData.sensor2 < lineCalMin){lineCalMin=lineSensorData.sensor2;}
        //if (lineSensorData.sensor3 < lineCalMin){lineCalMin=lineSensorData.sensor3;}
        //if (lineSensorData.sensor4 < lineCalMin){lineCalMin=lineSensorData.sensor4;}
        //if (lineSensorData.sensor5 < lineCalMin){lineCalMin=lineSensorData.sensor5;}
        //sei();
      }        

      break;

      case ROBOTFUNCTION_LINEFOLLOWER:
        readLineSensors();
      break;

      case ROBOTFUNCTION_WALLFOLLOWER:
        readWallSensors();
      break;
    }
  }
}

void SystemReset()
{
  wdt_enable(WDTO_120MS); // set watchdog timer
  cli(); // disable interrupts
  while(1){} // wait for watchdog to reset processor
}

void WDT_off()
{
  cli();
  wdt_reset();
  /* Clear WDRF in MCUSR */
  MCUSR &= ~(1<<WDRF);
  /* Write logical one to WDCE and WDE */
  /* Keep old prescaler setting to prevent unintentional time-out
  */
  WDTCSR |= (1<<WDCE) | (1<<WDE);
  /* Turn off WDT */
  WDTCSR = 0x00;
  sei();
  
}

void _mode_init()
{
  GO = 0;
  //wallCalMin = 0;
  //wallcalMax = 255;
  //lineCalMin = 0;
  //lineCalMax = 255;
  robotSpeed.LeftSpeed = 0;
  robotSpeed.RightSpeed = 0;  
  switch(mode.robotFunction)
  {
    case ROBOTFUNCTION_WAITFORUSER:
      stopFloorSensorMode();
    break;

    case ROBOTFUNCTION_DRAGRACE:
      startFloorSensorMode();
    break;

    case ROBOTFUNCTION_LINEFOLLOWER:
      startFloorSensorMode();
    break;

    case ROBOTFUNCTION_WALLFOLLOWER:
      stopFloorSensorMode();
    break;


  }
}


int dragcount=0;
int FinishLineCrossed = 0;
#define AMeter 2857
int draglastmillis = 0;
int dragstartmillis = 0;
int dragphase = 0;
int Accel = 100;
int speed = 10;
int trim = 0;
void DragGo()
{
  //2857 = 1 meter
  //int dist = URobotDistance.robotDistance.LeftDistance;
  blink(1<<LED1,1, blinkSpeed_50,blinkSpeed_50); //delay and heartbeat
//   if (millis()> draglastmillis+100)
//   {
    getLinePos();
    trimToLine();
    robotSpeed.LeftSpeed = speed-trim;
    robotSpeed.RightSpeed = speed+trim;
    //draglastmillis = millis();
    
    if(Accel)
    {
      speed += 3;
      Accel--;
    }
    else
    {
      if (lineSensorData.sensor1 > lineMidPoint || lineSensorData.sensor5 > lineMidPoint)
      {
      FinishLineCrossed = 1;
      }
      
      if (FinishLineCrossed)
      {
          robotSpeed.LeftSpeed = -20;
          robotSpeed.RightSpeed = -20;
          _delay_ms(250);
          SystemReset();
      }

    }        
}
// 

